/**
 * @file /src/qnode.cpp
 *
 * @brief Ros communication central!
 *
 * @date February 2011
 **/

/*****************************************************************************
** Includes
*****************************************************************************/

#include <ros/ros.h>
#include <ros/network.h>
#include <string>
#include <std_msgs/String.h>
#include <sstream>
#include "../include/alphadog_monitor/qnode.hpp"



/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace alphadog_monitor {

/*****************************************************************************
** Implementation
*****************************************************************************/

QNode::QNode(int argc, char** argv ) :
    init_argc(argc), init_argv(argv)
{
    QSettings main_setting("alphadog_monitor","settings");

    // 控制话题
    JointCmdTopic  = main_setting.value("driver/joints_driver", "/quadruped/qt_monitor/joints_cmd").toString();
    FootCmdTopic   = main_setting.value("driver/feet_driver", "/quadruped/qt_monitor/feet_cmd").toString();
    JoystickCmdTopic = main_setting.value("command/joystick", "/quadruped/qt_monitor/joystick_cmd").toString();
    StatusCmdTopic = main_setting.value("command/status", "/quadruped/qt_monitor/status_cmd").toString();

    // 图像话题
    InitImgTopic    = main_setting.value("image/init_img","/quadruped/qt_monitor/init_img").toString();
    HandledImgTopic = main_setting.value("image/handled_img", "/quadruped/qt_monitor/handled_img").toString();

    // 反馈数据
    ImuDataTopic    = main_setting.value("driver/pose_data", "/quadruped/qt_monitor/pose_data").toString();
    FeetDataTopic   = main_setting.value("driver/feet_data", "/quadruped/qt_monitor/feet_data").toString();

    // 曲线绘制数据
    GraphTopic      = main_setting.value("graph/data", "/quadruped/qt_monitor/graph_data").toString();

}

QNode::~QNode()
{
    if(ros::isStarted()) {
      ros::shutdown(); // explicitly needed since we use ros::start();
      ros::waitForShutdown();
    }
	wait();
}

bool QNode::init() {
	ros::init(init_argc,init_argv,"alphadog_monitor");
	if ( ! ros::master::check() ) {
		return false;
	}
	ros::start(); // explicitly needed since our nodehandle is going out of scope.
	ros::NodeHandle n;

    // 启动节点
    SubAndPubTopic();
	start();
    run();
	return true;
}

bool QNode::init(const std::string &master_url, const std::string &host_url) {
	std::map<std::string,std::string> remappings;
    remappings["__master"] = master_url;
    remappings["__hostname"] = host_url;
//    remappings["__master"] = "http://192.168.103.57:11311";
//    remappings["__hostname"] = "192.168.103.106";
	ros::init(remappings,"alphadog_monitor");
	if ( ! ros::master::check() ) {
		return false;
	}
    ros::start(); // explicitly needed since our nodehandle is going out of scope.
    // Add your ros communications here.
    SubAndPubTopic();
    start();
	return true;
}

void QNode::run()
{
    ros::MultiThreadedSpinner s(4);
    ros::spin(s);
}

void QNode::log( const LogLevel &level, const std::string &msg) {
	logging_model.insertRows(logging_model.rowCount(),1);
	std::stringstream logging_model_msg;
	switch ( level ) {
		case(Debug) : {
				ROS_DEBUG_STREAM(msg);
				logging_model_msg << "[DEBUG] [" << ros::Time::now() << "]: " << msg;
				break;
		}
		case(Info) : {
				ROS_INFO_STREAM(msg);
				logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
				break;
		}
		case(Warn) : {
				ROS_WARN_STREAM(msg);
				logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
				break;
		}
		case(Error) : {
				ROS_ERROR_STREAM(msg);
				logging_model_msg << "[ERROR] [" << ros::Time::now() << "]: " << msg;
				break;
		}
		case(Fatal) : {
				ROS_FATAL_STREAM(msg);
				logging_model_msg << "[FATAL] [" << ros::Time::now() << "]: " << msg;
				break;
		}
	}
	QVariant new_row(QString(logging_model_msg.str().c_str()));
	logging_model.setData(logging_model.index(logging_model.rowCount()-1),new_row);
	Q_EMIT loggingUpdated(); // used to readjust the scrollbar
}

/**
 * @brief 定义发布者和接收者
 */
void QNode::SubAndPubTopic()
{
    ros::NodeHandle n;

    // 控制指令发布者
    JointCmdPuber   = n.advertise<sensor_msgs::JointState>(JointCmdTopic.toStdString(),10);
    FootCmdPuber    = n.advertise<sensor_msgs::JointState>(FootCmdTopic.toStdString(),10);
    JoystickCmdPuber = n.advertise<sensor_msgs::JointState>(JoystickCmdTopic.toStdString(),10);
    StatusCmdPuber  = n.advertise<geometry_msgs::Pose>(StatusCmdTopic.toStdString(),10);


    // 图像订阅者
    InitImgSuber    = n.subscribe(InitImgTopic.toStdString(), 10, &QNode::InitImgCallback, this);
    HandledImgSuber = n.subscribe(HandledImgTopic.toStdString(), 10, &QNode::HandledImgCallback, this);

    // 反馈数据订阅
    FeetSuber       = n.subscribe(FeetDataTopic.toStdString(), 10, &QNode::FeetDataReceiveCallback, this);
    ImuSuber        = n.subscribe(ImuDataTopic.toStdString(), 10, &QNode::ImuDataReceiveCallback, this);
    GraphSuber      = n.subscribe(GraphTopic.toStdString(), 10, &QNode::GraphDataReceiveCallback, this);

    // 路径规划相关
    RouteDataPuber = n.advertise<sensor_msgs::JointState>("/quadruped/control/route_data",10);
    StateSuber = n.subscribe("/quadruped/control/state_data", 10, &QNode::StateDataCallback, this);
}

// 状态接收回调函数
void QNode::StateDataCallback(const sensor_msgs::JointStateConstPtr& msg){
    for(int i = 0; i < 6; i ++){
        state[i] = msg->position[i];
    }
    emit updateState(state);
}

//图像话题的回调函数
void QNode::InitImgCallback(const sensor_msgs::CompressedImageConstPtr& msg)
{
  cv_bridge::CvImagePtr cv_ptr;
  try {
    //深拷贝转换为opencv类型
    cv_bridge::CvImagePtr cv_ptr_compressed =
        cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    QImage im = Mat2QImage(cv_ptr_compressed->image);
    emit Show_image(0, im);
  } catch (cv_bridge::Exception& e) {
    log(Error, ("InitImg exception: " + QString(e.what())).toStdString());
    return;
  }
}

void QNode::HandledImgCallback(const sensor_msgs::CompressedImageConstPtr& msg) {
  cv_bridge::CvImagePtr cv_ptr;
  try {
    //深拷贝转换为opencv类型
    cv_bridge::CvImagePtr cv_ptr_compressed =
        cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    QImage im = Mat2QImage(cv_ptr_compressed->image);
    emit Show_image(1, im);
  } catch (cv_bridge::Exception& e) {
    log(Error, ("HandledImg exception: " + QString(e.what())).toStdString());
    return;
  }
}

/**
 * @brief 足端数据接收回调
 * @param msg
 */
void QNode::FeetDataReceiveCallback(const sensor_msgs::JointStatePtr& msg)
{

}

/**
 * @brief 姿态数据接收回调
 * @param msg
 */
void QNode::ImuDataReceiveCallback(const sensor_msgs::ImuPtr& msg)
{

}

/**
 * @brief 曲线绘制数据接收回调
 * @param msg
 */
void QNode::GraphDataReceiveCallback(const sensor_msgs::JointStateConstPtr& msg){

    for(int i = 0; i < 12; i ++){
        debug_val[i] = msg->position[i];
        debug_name[i] = QString::fromStdString(msg->name[i]);
    }
    emit updateGraph(debug_val, debug_name);
}

/**
 * @brief 将矩阵转换为QT图像
 * @param src
 * @return QImage
 */
QImage QNode::Mat2QImage(cv::Mat const& src) {
  QImage dest(src.cols, src.rows, QImage::Format_ARGB32);

  const float scale = 255.0;

  if (src.depth() == CV_8U) {
    if (src.channels() == 1) {
      for (int i = 0; i < src.rows; ++i) {
        for (int j = 0; j < src.cols; ++j) {
          int level = src.at<quint8>(i, j);
          dest.setPixel(j, i, qRgb(level, level, level));
        }
      }
    } else if (src.channels() == 3) {
      for (int i = 0; i < src.rows; ++i) {
        for (int j = 0; j < src.cols; ++j) {
          cv::Vec3b bgr = src.at<cv::Vec3b>(i, j);
          dest.setPixel(j, i, qRgb(bgr[2], bgr[1], bgr[0]));
        }
      }
    }
  } else if (src.depth() == CV_32F) {
    if (src.channels() == 1) {
      for (int i = 0; i < src.rows; ++i) {
        for (int j = 0; j < src.cols; ++j) {
          int level = scale * src.at<float>(i, j);
          dest.setPixel(j, i, qRgb(level, level, level));
        }
      }
    } else if (src.channels() == 3) {
      for (int i = 0; i < src.rows; ++i) {
        for (int j = 0; j < src.cols; ++j) {
          cv::Vec3f bgr = scale * src.at<cv::Vec3f>(i, j);
          dest.setPixel(j, i, qRgb(bgr[2], bgr[1], bgr[0]));
        }
      }
    }
  }

  return dest;
}

/**
 * @brief 将QT图像转换为矩阵
 * @param image
 * @return cv::Mat
 */
cv::Mat QNode::QImage2Mat(QImage& image) {
  cv::Mat mat;
  switch (image.format()) {
    case QImage::Format_ARGB32:
    case QImage::Format_RGB32:
    case QImage::Format_ARGB32_Premultiplied:
      mat = cv::Mat(image.height(), image.width(), CV_8UC4,
                    (void*)image.constBits(), image.bytesPerLine());
      break;
    case QImage::Format_RGB888:
      mat = cv::Mat(image.height(), image.width(), CV_8UC3,
                    (void*)image.constBits(), image.bytesPerLine());
      cv::cvtColor(mat, mat, CV_BGR2RGB);
      break;
    case QImage::Format_Indexed8:
      mat = cv::Mat(image.height(), image.width(), CV_8UC1,
                    (void*)image.constBits(), image.bytesPerLine());
      break;
    default: break;
  }
  return mat;
}

}  // namespace alphadog_monitor
